#include "drv_motor.h"
#include "user_include.h"

int Pwm;
MOTOR_CMDDATA_def mCmd;
MOTOR_POSSET_def mPset;
MOTOR_POS_INFO_def mPos;
MOTOR_RUNs_def mRun;
MOTOR_STOPs_def mStop;
MOTOR_FORCs_def mForc;
MOTOR_SPEEDs_def mSpd;

const u32 M_PROID[3] = {SPD0_PROID,SPD1_PROID,SPD2_PROID};

static u32 PercentToPosition(u8 p)
{
    u32 temp;
	if(p == 0)
	{
		return mPos.down;
	}
	else if(p == 100)
	{
		return mPos.up;
	}
	else
	{
		temp = p * (mPos.up - mPos.down);
		temp /=100;
		temp += mPos.down;
		return temp;
	}
}

static u8 PositionToPercent(u32 pos)
{
	u32 temp;

	if(mStop.percent)
	    return mStop.last_percent;

    if(pos <= mPos.down)
	    return 0;
    
	if(pos >= mPos.up)
	    return 100;

	temp = (pos - mPos.down) * 100;
	temp =temp/(mPos.up - mPos.down);
	return temp;
}

u8 Motor_Get_LocPercent(void)
{
    u8 tmp;
    if(mPset.set == 0)
        return 0xff;
    tmp = PositionToPercent(mMem.pos0);
    if(mMem.config.dir)
        return (100 - tmp);
    else
        return tmp;
}

static void motor_run_init(void)
{   
    Lpm_SetTime_4S();
    
    mStop.percent = STOP_Normal;
    mStop.third = 0;
    mStop.state = 0;

    mRun.state = MR_S_RUN;
    mPos.run_pos = 0;
    Pwm = PWM_INIT;

    if(RO_VHALL() == 0)
    {
        VHALL_ON();
        Delay_nMs(10);
        
        Gpio_ClearIrq(HALL1_PORT, HALL1_PIN);
        Gpio_EnableIrq(HALL1_PORT, HALL1_PIN, GpioIrqFalling);
        Gpio_EnableIrq(HALL1_PORT, HALL1_PIN, GpioIrqRising);

        Gpio_ClearIrq(HALL2_PORT, HALL2_PIN);
        Gpio_EnableIrq(HALL2_PORT, HALL2_PIN, GpioIrqFalling);
        Gpio_EnableIrq(HALL2_PORT, HALL2_PIN, GpioIrqRising);
    }
    BREAK_OFF();
    Delay_nMs(30);

    mSpd.thall = 0;
    mSpd.set = M_PROID[mMem.config.spd];
}

static void m_up(void)
{
    mRun.way = MR_WAY_UP;
    motor_run_init();
    UpPWM_SET(Pwm);
}

static void m_dn(void)
{
    mRun.way = MR_WAY_DOWN;
    motor_run_init();
    DnPWM_SET(Pwm);
}

static void Stop(void)
{  
    Lpm_SetTime_4S();
    
    DnPWM_SET(0);
    UpPWM_SET(0);

    Delay_nMs(50);
    //BREAK_ON();

    mRun.state = MR_S_STOPR;
    mRun.type = MR_TYPE_Normal;
}

void Motor_Stop(void)
{
    switch(mRun.state)
    {
        case MR_S_RUN:
            Stop();
            mRun.state = MR_S_STOP;
            break;
        case MR_S_STOPR:
            mRun.state = MR_S_STOP;
            break;
        case MR_S_STOP:
        default:
            break;
    }

    mRun.time_01s = 0;
    printf("-MSTOP-p:%x-",mMem.pos0);
    printf("-t:%d-",mSpd.thall);
    printf("-miv:%d-",mSpd.miv);
    printf("-mic:%d-",mSpd.mic);
    printf("-bat:%d-usb:%d--%d", vBat, vUsb5, sBat);
}

static u8 RunTo_Up(u32 pos)
{
    if(mStop.state == STOP_Up)
        return 1;
    if(mRun.state != MR_S_RUN)
    {
        m_up();
    }
    else if(mRun.way == MR_WAY_DOWN)
    {
        Stop();
        Delay_nMs(300);
        return 0;
    }
    
    if(pos)
        mPos.run_to_pos = mMem.pos0 + pos;
    else
        mPos.run_to_pos = mPos.up;
    return 1;
}

static u8 RunTo_Dn(u32 pos)
{
    if(mStop.state == STOP_Dn)
        return 1;
    if(mRun.state != MR_S_RUN)
    {
        m_dn();
    }
    else if(mRun.way == MR_WAY_UP)
    {
        Stop();
        Delay_nMs(300);
        return 0;
    }

    if(pos)
        mPos.run_to_pos = mMem.pos0 - pos;
    else
        mPos.run_to_pos = mPos.down;
    return 1;
}

static u8 RunTo_Third(void)
{
    if(mStop.third||(mMem.ps.P3 == 0))
        return 1;

    mPos.run_to_pos = mMem.pos3;
    if(mMem.pos0<mMem.pos3)
    {
        if(mRun.state != MR_S_RUN)
        {
            m_up();
        }
        if(mRun.way == MR_WAY_DOWN)
        {
            Stop();
            Delay_nMs(300);
            return 0;
        }
    }
    else if(mMem.pos0>mMem.pos3)
    {
        if(mRun.state != MR_S_RUN)
        {
            m_dn();
        }
        if(mRun.way == MR_WAY_UP)
        {
            Stop();
            Delay_nMs(300);
            return 0;
        }
    }
    else
    {
        mStop.third = 1;
        Motor_Stop();
    }

    mRun.type = MR_TYPE_Third;
    return 1;
}

static u8 RunTo_Percert(void)
{
    if(mPset.set == 0)
        return 1;
    if(mStop.percent&&(mStop.last_percent==mCmd.data))
        return 1;
    
    mPos.run_to_pos = PercentToPosition(mCmd.data);
    if(mMem.pos0<mPos.run_to_pos)
    {
        if(mRun.state != MR_S_RUN)
        {
            m_up();
        }
        if(mRun.way == MR_WAY_DOWN)
        {
            Stop();
            Delay_nMs(300);
            return 0;
        }
    }
    else if(mMem.pos0>mPos.run_to_pos)
    {
        if(mRun.state != MR_S_RUN)
        {
            m_dn();
        }
        if(mRun.way == MR_WAY_UP)
        {
            Stop();
            Delay_nMs(300);
            return 0;
        }
    }
    else
    {
        mStop.percent = 1;
        Motor_Stop();
    }

    mStop.last_percent = mCmd.data;
    mRun.type = MR_TYPE_Percent;
    return 1;
}

void Motor_SetCmd(MOTOR_CMD_def cmd,u8 data)
{   
    if(mMem.config.dot)
    {
        switch(cmd)
        {
            case mc_up:
                cmd = mc_up_dot;
                break;
            case mc_dn:
                cmd = mc_dn_dot;
                break;
            case mc_up_2s:
                cmd = mc_up;
                break;
            case mc_dn_2s:
                cmd = mc_dn;
                break;
        }
    }
    
    if(mMem.config.dir)
    {
        switch(cmd)
        {
            case mc_up:
                cmd = mc_dn;
                break;
            case mc_dn:
                cmd = mc_up;
                break;
            case mc_up_dot:
                cmd = mc_dn_dot;
                break;
            case mc_dn_dot:
                cmd = mc_up_dot;
                break;
            case mc_percent:
                if(data>100)
                    return;
                data = 100 - data;
                break;
            default:
                break;
        }
    }

	if((cmd == mc_percent)&&(mPset.set == 0))
    {
        if(data == 0)
            cmd = mc_dn;
        else if(data == 100)
            cmd = mc_up;
    }
	
    mCmd.cmd = cmd;
    mCmd.data = data;
}

void Motor_Contrl_10ms(void)
{
    switch(mCmd.cmd)
    {
        case mc_stop:
            Motor_Stop();
            break;
        case mc_up:
            if(RunTo_Up(0))
                break;
            else
                return;
        case mc_dn:
            if(RunTo_Dn(0))
                break;
            else
                return;
        case mc_up_dot:
            if(RunTo_Up(DOT_POS))
                break;
            else
                return;
        case mc_dn_dot:
            if(RunTo_Dn(DOT_POS))
                break;
            else
                return;
        case mc_one:
            if(mRun.state == MR_S_RUN)
            {
                mCmd.cmd = mc_stop;
                return;
            }
            else if(mRun.way == MR_WAY_UP)
            {
                mCmd.cmd = mc_dn;
                return;
            }
            else
            {
                mCmd.cmd = mc_up;
                return;
            }
        case mc_third:
            if(RunTo_Third())
                break;
            else
                return;
        case mc_percent:
            if(RunTo_Percert())
                break;
            else
                return;
        case mc_notice:
            if(mRun.state == MR_S_RUN)
                return;
            if(mCmd.data == 0)
            {
                mRun.notic = 0;
                break;
            }
            if(mRun.notic == 0)
            {
                mRun.notic = 1;
                if(mMem.pos0>(mPos.down+NOTICE_POS))
                    RunTo_Dn(NOTICE_POS);
                else
                    RunTo_Up(NOTICE_POS);
            }
            else 
            {
                Delay_nMs(500);
                if(mRun.way == MR_WAY_UP)
                    RunTo_Dn(NOTICE_POS);
                else
                    RunTo_Up(NOTICE_POS);
            }
            mCmd.data --;
            return;
        default:
            break;
    }
    mCmd.cmd = mc_idle;
}

void Motor_CheckPos_1ms(void)
{
    u8 f = 0;
    if(mRun.state != MR_S_RUN)
        return;
    if(mRun.way == MR_WAY_UP)
    {
        if(mMem.pos0 >= mPos.up)
        {
            f = 1;
            mStop.state = STOP_Up;
        }
        else if(mMem.pos0 >= mPos.run_to_pos)
            f = 1;
    }
    else
    {
        if(mMem.pos0 <= mPos.down)
        {
            f = 1;
            mStop.state = STOP_Dn;
        }
        else if(mMem.pos0 <= mPos.run_to_pos)
            f = 1;
    }

    if(f)
    {
        if(mRun.type == MR_TYPE_Third)
        {
            mStop.third = 1;
        }
        else if(mRun.type == MR_TYPE_Percent)
        {
            mStop.percent = 1;
        }
        Motor_Stop();
    }
}

#define ADMI_NMAX   8
void Motor_CheckMi_10ms(void)
{
    static u32 ad_buf[ADMI_NMAX];
    static u8 ad_n = 0;
    u32 sum = 0;
    u8 i;
    mSpd.mic = Get_Adc(ADMI_CH);
    ad_buf[ad_n] = mSpd.mic;
    ad_n ++;
    if(ad_n>=ADMI_NMAX)
        ad_n = 0;
    for(i=0;i<ADMI_NMAX;i++)
        sum += ad_buf[i];
    mSpd.miv = sum>>3;
    if((mSpd.miv>MI_AVG_MAX)||(mSpd.mic>MI_CUR_MAX))
    {
        Motor_Stop();
        mForc.count ++;
        mForc.pos = mMem.pos0;
        mForc.way = mRun.way;
    }
}

void Motor_CheckSpd_100ms(void)
{
    int x;
    if(mRun.state != MR_S_RUN)
        return;
    mRun.time_01s ++;
    if(mRun.time_01s>=MRUN_TIME_MAX)
    {
        Motor_Stop();
        return;
    }
    if(mSpd.thall>=MTHALL_MAX)
    {
        Motor_Stop();
        return;
    }
    
    x = mSpd.set - mSpd.proid;
    x /= PROID_X;
    if(x>PWM_X)
        x = PWM_X;
    else if(x<(-PWM_X))
        x = -PWM_X;
    Pwm -= x;
    if(Pwm>PWM_MAX)
        Pwm = PWM_MAX;
    else if(Pwm<PWM_MIN)
        Pwm = PWM_MIN;
    if(mRun.way == MR_WAY_UP)
        UpPWM_SET(Pwm);
    else
        DnPWM_SET(Pwm);
    
    //printf("s:%d-t:%d",mSpd.set,mSpd.proid);
    //printf("-pwm:%d",Pwm);
}

